# ##Planner.py

from map import Map

class Planner:
	rbInitialX = 1
	rbInitialY = 1
	wallfollowing = True
	count = 0

	def __init__(self, wallFollow):
		self.rbInitialX = 1
		self.rbInitialY = 1
		self.wallfollowing = wallFollow

	def getNextAction(self, robot_map):
		if isinstance(robot_map, Map):
			if self.wallfollowing:
				action = self.wallFollowing(robot_map)
			else:
				action = self.searchAdjacentArea(robot_map)
			if action < 0:
				action = self.searchNewArea(robot_map)
			else:
				action = [action]
			return action
		else:
			print("ERROR: robot_map is not of type Map")

	def searchAdjacentArea(self, robot_map):
		x = robot_map.robotX
		y = robot_map.robotY
		dx, dy = robot_map.getDirArrayBasedOnOrientation()
		# right up left
		if robot_map.map[x + dx[1]][y + dy[1]] == 0:
			return 1
		elif robot_map.map[x + dx[0]][y + dy[0]] == 0:
			return 0
		elif robot_map.map[x + dx[3]][y + dy[3]] == 0:
			return 3
		else:
			return -1

	def searchNewArea(self, robot_map):
		x = robot_map.robotX
		y = robot_map.robotY

		dx, dy = robot_map.getDirArrayBasedOnOrientation()
		tested = [[0 for j in range(robot_map.col_size)] for i in range(robot_map.row_size)]
		l = []
		action = []
		l.append([x, y, 0, -1])
		tested[x][y] = 1
		h = 0

		hasFound = False
		while h < len(l) and hasFound == False:
			x = l[h][0]
			y = l[h][1]
			for index in range(4):
				tempX = x + dx[index]
				tempY = y + dy[index]
				if (robot_map.map[tempX][tempY] >= 0) and (tested[tempX][tempY] == 0):
					l.append([tempX, tempY, h, index])
					tested[tempX][tempY] = 1
					if robot_map.map[tempX][tempY] == 0:
						hasFound = True
						break
			h += 1

		if hasFound:
			# successfully find the next target cell
			h = len(l) - 1
			while not (l[h][3] == -1):
				action.append(l[h][3])
				h = l[h][2]

		return action[::-1]  # reverse the list

	def wallFollowing(self, robot_map):
		x = robot_map.robotX
		y = robot_map.robotY
		dx, dy = robot_map.getDirArrayBasedOnOrientation()
		#left, up, right, down, left ...
		action = -1
		if robot_map.map[x + dx[3]][y + dy[3]] >= 0:
			action = 3
		elif robot_map.map[x + dx[0]][y + dy[0]] >= 0:
			action = 0
		elif robot_map.map[x + dx[1]][y + dy[1]] >= 0:
			action = 1
		elif robot_map.map[x + dx[2]][y + dy[2]] >= 0:
			action = 2
		elif robot_map.map[x + dx[3]][y + dy[3]] > 0:
			action = 3
		elif robot_map.map[x + dx[0]][y + dy[0]] > 0:
			action = 0
		elif robot_map.map[x + dx[1]][y + dy[1]] > 0:
			action = 1
		elif robot_map.map[x + dx[2]][y + dy[2]] > 0:
			action = 2
		else:
			action = -1
		if action != -1:
			if x + dx[action] == self.rbInitialX and y + dy[action] == self.rbInitialY:
				if robot_map.map[self.rbInitialX + 1][self.rbInitialY] != 0:
					if robot_map.map[self.rbInitialX][self.rbInitialY + 1] != 0:
						if robot_map.map[self.rbInitialX - 1][self.rbInitialY] != 0:
							if robot_map.map[self.rbInitialX][self.rbInitialY-1] != 0:
								action = -2
								self.wallfollowing = False
		return action
